
#include "global.h"


using namespace cv;
using namespace std;

Mat target_stack;
vector<Point2f> target_stack_vect;

double angular_vel = 0;
double linear_vel = 100;  // initial vel

char flg;

ArRobot robot;

ArPose pose;

vector<rb_sonar> rb_sonars;

decision deci;

Point2i target=Point2i(5000, 5000);


float getTargetPosition()
{
	/*
	char *centralhost="140.118.38.92";
	double pose[3];


	myIPC_Init(centralhost);
	Sleep(200);

	// get robot heading
	myGetRobotPose(pose);
	Sleep(200);
	printf("%f  %f  %f \n", pose[0], pose[1], pose[2]);

	// get delta angle
	// where's the function


	// do the minus

	myShutdown();	
	system("pause");
	*/


	pose = robot.getPose();

	double X;
	double Y;
	double th;
	pose.getPose(&X, &Y, &th);

	//printf("%0.2f, %0.2f, %0.2f\n", pose.getX(), pose.getY(), pose.getTh());
	//printf("%0.2f, %0.2f, %0.2f\n", *X, *Y, *th);
	//printf("%0.2f, %0.2f, %0.2f\n", X, Y, th);


	return 0;
}


rb_sonar getObstaclePosition() // split this module if getting bigger
{	
	//double ret_val = 1;

	// Get all sonars in range
	rb_sonar sn, ret_sn;
	rb_sonars.clear();
	//int test;

	//test = robot.getSonarRange(1);
	//printf("%d, ", test);

	for (int i=0; i<SONAR_NUM; i++)
	{	 
		//val = robot.getSonarRange(i);
		sn.value = (double)robot.getSonarRange(i);
		//sn.value = (double)test;
		//sn.value = robot.getSonarRange(i);

	  	if (sn.value >= OBS_DIST_THRES) sn.value = INF;

		sn.idx = i;

		rb_sonars.push_back(sn);

		// sonar sensor probe
		//printf("%0.3f, ", sn.value);
		//printf("%d, ", test);
	}	
	//printf("\n");

	// get min Left
	rb_sonar sn_minL = rb_sonars[0];
	if (rb_sonars[1].value < sn_minL.value ) sn_minL = rb_sonars[1];
	if (rb_sonars[2].value < sn_minL.value ) sn_minL = rb_sonars[2];
	if (rb_sonars[7].value < sn_minL.value ) sn_minL = rb_sonars[7];   // lack of this - prog bug!

	// get min Right
	rb_sonar sn_minR = rb_sonars[3];
	if (rb_sonars[4].value < sn_minR.value ) sn_minR = rb_sonars[4];
	if (rb_sonars[5].value < sn_minR.value ) sn_minR = rb_sonars[5];
	if (rb_sonars[6].value < sn_minR.value ) sn_minR = rb_sonars[6];

	// minL vs minR
	// exception A: block one side (L/R)
	const double tolA = 10;
	//if ((sn_minL.idx == 2) && (sn_minR.idx == 3) && (sn_minL.value + tolA >= sn_minR.value) && ())
	if ((sn_minL.idx == 2) && (sn_minR.idx == 3) && (abs(sn_minL.value - sn_minR.value) <= tolA))
	{
		// choose the open side to go
		double sn_L_total = rb_sonars[0].value + rb_sonars[1].value + rb_sonars[2].value;
		double sn_R_total = rb_sonars[3].value + rb_sonars[4].value + rb_sonars[5].value;
		
		if (sn_L_total >= sn_R_total)
		{
			ret_sn = sn_minL;
		} 
		else
		{
			ret_sn = sn_minR;
		}
	}
	else
	{
	// exception B: block U shape
	// later!!
	
	// normal case
	if (sn_minL.value < sn_minR.value) 
		//ret_val = sn_minL.value;
		ret_sn = sn_minL;
	else 
		ret_sn = sn_minR;
	}

	// return value;
	//return sonar_ang[ret_sn.idx]/PI*180;	
	//printf("obs pos = %0.2f, %d", ret_sn.value, ret_sn.idx);
	return ret_sn;
}

void steering(double steering_param, double velocity_param)
{
	double K = steering_param;

	/*
	if (steering_mode == DECEL_LROT || steering_mode == DECEL_RROT || steering_mode == DECEL_STRT)
	{
		linear_vel = linear_vel - K*DEL_LINEAR_VEL;

		if (steering_mode != DECEL_STRT)
		{
			angular_vel = angular_vel - K*DEL_ANGULAR_VEL;
		}
	}
	else
		if (steering_mode == ACCEL_LROT || steering_mode == ACCEL_RROT || steering_mode == ACCEL_STRT)
		{
			linear_vel = linear_vel + K*DEL_LINEAR_VEL;

			if (steering_mode != DECEL_STRT)
			{
				angular_vel = angular_vel + K*DEL_ANGULAR_VEL;
			}
		}
	*/


	//printf("steer=%0.2f;vel= %0.2f;", steering_param, velocity_param);

	// cur_rb_vel = cur_rb_vel + dvel;
	// cur_rb_heading = cur_rb_heading + dtheta;

	//linear_vel = linear_vel + velocity_param;     // WORST BUG!!!!!!!
	//angular_vel = angular_vel + steering_param;
	linear_vel = velocity_param;
	angular_vel = steering_param;

	//printf("lin=%0.2f, ang=%0.2f\n", linear_vel, angular_vel);
	


	double V = linear_vel;
	double dtheta = angular_vel;
	//double V = 100;
	//double dtheta = -PI/2;

	double wr = (2*V+dtheta*L)/2/r;
	double wl = (2*V-dtheta*L)/2/r;

	// delimiter
	if (abs(wr) >= MAX_WHEEL_VEL) wr = wr/abs(wr)*MAX_WHEEL_VEL;
	if (abs(wl) >= MAX_WHEEL_VEL) wl = wl/abs(wl)*MAX_WHEEL_VEL;
	//printf("wr=%0.2f,wl=%0.2f\n", wr, wl);

	//robot.setVel2(1*PI*180/PI,1*PI*180/PI); // sim setVel2(R, L) !!!!!
	//robot.setVel2(-wl*180/PI,-wr*180/PI);
	//robot.setVel2(wr*180/PI, wl*180/PI);   // good!!
	//robot.setVel2(wl*180/PI,wr*180/PI);
	robot.setVel(200);
	robot.setDeltaHeading(5*dtheta/PI*180);
	//robot.setDeltaHeading(-5);

/*
	switch(steering_mode)  
	{
	case (DECEL_LROT):
		cout << "Run Away!";
		break;

	case (DECEL_RROT):
		cout << "Fight Defensively";
		break;

	case (DECEL_STRT):

		break;

	case (ACCEL_LROT):
		cout << "Run Away!";
		break;

	case (ACCEL_RROT):
		cout << "Fight Defensively";
		break;

	case (ACCEL_STRT):

		cout << "All Out Attack!";
		break;

	} // end switch
*/
}


void controller()
{

	// go straight to target
	

	// store a stack of targets


	// create new target if current target is blocked

}


void wrappedOnMouse(int event, int x, int y, int flags, void* ptr)
{
	TargetFeeder* mcPtr = (TargetFeeder*)ptr;
	if(mcPtr != NULL)
		mcPtr->on_mouse(event, x, y, flags, 0);
}


void FeedingFromClick()
{

	Mat image;
	char c;
	char* file = "expmap.jpg";
	image = imread(file, 1);


	// get obstacle positions
	int* out;
	int m, n;
	//	myGetObstacleCoord(&out, m, n);

	//	Mat obstacles = Mat::zeros(1, m*n/2, CV_32FC2);
	// CREATE RANDOM OBSTACLES
	RNG rng; 
	m = 2; n = 50;
	out=(int*)malloc(m*n*sizeof(int));
	for (int i=0; i<m*n; i++)
	{
		out[i] = rng.uniform(0, image.rows);
		//out[i] = i;
		//out++;
	}

	Mat obstacles = Mat::zeros(1, m*n/2, CV_32FC2);

	for (int i=0; i<m*n; i++)
	{
		if(i%2==0)
		{
			printf("%5d",out[i]);
			obstacles.at<Vec2f>(0,i/2)[0] = out[i];
		}
		else
		{
			printf(" %5d ",out[i]);
			obstacles.at<Vec2f>(0,i/2)[1] = out[i];
		}
	}

	TargetFeeder tf = TargetFeeder(image, obstacles);

	char* wind_name = "Choose Workspace";
	//	char c;

	// draw obstacles on image
	drawCircles(tf.map_image, tf.map_obstacles);



	imshow(wind_name, tf.map_image);

	setMouseCallback(wind_name, wrappedOnMouse, (void*)&tf);

	//setMouseCallback(wind_name, on_mouse, 0);   // bug is here! cannot use mouseCallback

	// click 2 corners of workspace

	while (tf.clicked_points.size() < 2)
	{
		printf("Please click 2 POINTS to create workspace!!\n");

		c = cvWaitKey(1);
		if (c == 27) 
		{
			break;
		}
	}

	//	};

	// call generate
	tf.generate(10);

	// visualize trajectory
	tf.visualize();

}


int main(int argc, char* argv[])
{
	
	// Initialization
    float	target_pos, obstacles_pos, potential; // values for input variables
    char	option;	// var for selection of what user wants to do
	int ret_val;

	double steering_param, velocity_param;
	
	char c;

	
	//target_stack = Mat::zeros() -> use a  dynamic vector
	
	// Feeding targets
//	FeedingFromClick();

	// Init Fuzzy Controller
	FuzzyController FZCtrl(ret_val); 

	if (ret_val < 0)
	{
		cout << "Error Opening fuzzycontroller.fcl";
		// make sure the "working directory" in "Project | Settings..."
		// is set to the executable's directory if running from the MSVC IDE
	}


//***********************Aria Procedures*********************************************

		// Initialize some global data
	Aria::init();
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();

//*********************Simulation or Link to real Robot******************************

	char charBuffer[100];
	//"140.118.38.108": Link to P3-DX
	//"140.118.38.109": Link to Amigo
	sprintf(charBuffer, "-rh %s", "140.118.38.109");
	parser.addDefaultArgument(charBuffer);//if you want to using MobileSim, comment this line

//*********************Simulation or Link to real Robot******************************
	
	//ArRobot robot;

	ArRobotConnector robotConnector(&parser, &robot);
	if (!robotConnector.connectRobot())
	{
		if (!parser.checkHelpAndWarnUnparsed())
		{
			ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything");
		}
		else
		{
			ArLog::log(ArLog::Terse, "Error, could not connect to robot.");
			Aria::logOptions();
			Aria::exit(1);
		}
	}


	if(!robot.isConnected())
	{
		ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!");
	}

	// SL added
	// Connector for laser rangefinders
	//ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

	// Connector for compasses
	//ArCompassConnector compassConnector(&parser);


	// Parse the command line options. Fail and print the help message if the parsing fails
	// or if the help was requested with the -help option
	if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
	{    
		Aria::logOptions();
		Aria::exit(1);
		return 1;
	}

	// Used to access and process sonar range data
	ArSonarDevice sonarDev;
	// SL added
	
	// Used to perform actions when keyboard keys are pressed
	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);

	// ArRobot contains an exit action for the Escape key. It also 
	// stores a pointer to the keyhandler so that other parts of the program can
	// use the same keyhandler.
	//robot.attachKeyHandler(&keyHandler);
	printf("You may press escape to exit\n");

	// Attach sonarDev to the robot so it gets data from it.
	robot.addRangeDevice(&sonarDev);

	// Start the robot task loop running in a new background thread. The 'true' argument means if it loses
	// connection the task loop stops and the thread exits.
	robot.runAsync(true);

	/*
	if (!laserConnector.connectLasers(
		false,  // continue after connection failures
		false,  // add only connected lasers to ArRobot
		true    // add all lasers to ArRobot
		))
	{
		printf("Could not connect to lasers... exiting\n");
		Aria::exit(2);
	}
	*/

	// Create and connect to the compass if the robot has one.
	//ArTCM2 *compass = compassConnector.create(&robot);
	//if(compass && !compass->blockingConnect()) {
	//	compass = NULL;
	//}

	ArUtil::sleep(100);//unit: unsigned int ms
	
	// We need to lock the robot since we'll be setting up these modes
	// while the robot task loop thread is already running, and they 
	// need to access some shared data in ArRobot.
	//robot.lock();

	// now add all the modes for this demo
	// these classes are defined in ArModes.cpp in ARIA's source code.
	//ArModeLaser laser(&robot, "laser", 'l', 'L');
	//ArModeTeleop teleop(&robot, "teleop", 't', 'T');
	//ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U');
	//ArModeWander wander(&robot, "wander", 'w', 'W');
	//ArModePosition position(&robot, "position", 'p', 'P', &gyro);
	//ArModeSonar sonar(&robot, "sonar", 's', 'S');
	//ArModeBumps bumps(&robot, "bumps", 'b', 'B');
	//ArModeGripper gripper(&robot, "gripper", 'g', 'G');
	//ArModeCommand command(&robot, "command", 'd', 'D');
	//ArModeCamera camera(&robot, "camera", 'c', 'C');
	//ArModeIO io(&robot, "io", 'i', 'I');
	//ArModeActs actsMode(&robot, "acts", 'a', 'A');
	//ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass);

	// activate the default mode
	//teleop.activate();

	// turn on the motors
	robot.comInt(ArCommands::ENABLE, 1);

	//robot.unlock();

//==============================editing your code here=========================

	// Core servo flow
	//robot.setVel(200);			//unit: mm/sec
	//robot.setRotVel(100);			//unit: deg/sec (direction anticlock wise)
	//robot.setVel2(300, 100);		//unit: mm/sec

	//ArUtil::sleep(7000);			//unit: ms
	//robot.stop();

	//potential = 1;
	rb_sonar ret_obs_sn;
	float d0, d1;
	int d0_flg = 1;
	
	while (1)
	{ 
		// delay delta_t 

		// get obstacle status
		//int a = robot.getSonarRange(1);
		//printf("%d \n", a);
		//getchar();
		//Sleep(5);
		ret_obs_sn = getObstaclePosition();
		//obstacles_pos = (float)sonar_ang[ret_obs_sn.idx]/PI*180; -> bug bug
		//if (ret_obs_sn.value != INF)                               // (lack) logic bug -> this patch is another bug!!
			obstacles_pos = float(sonar_ang[ret_obs_sn.idx]);
		//else
		//	obstacles_pos = 0;
		//obstacles_pos = getObstaclePosition();
//		printf("obs dir = %0.2f; idx = %d\n", ret_obs_sn.value, ret_obs_sn.idx);
		//getchar();

		// get target status
		target_pos = getTargetPosition();
		//target_pos = 0;


		


		//########### CASE OBSTACLE EXISTS
		if (ret_obs_sn.value != INF)
		{
		// calculate potential
		d1 = ret_obs_sn.value;
		if (d0_flg) 
		{
			potential = 1;
			d0_flg = 0;
		}
		else 
			potential = d1 - d0;
		d0 = d1;

		printf("targetpos=%0.2f,obsdir=%0.2f,potential=%0.2f;", target_pos, obstacles_pos, potential);
		//printf("targetpos = %0.2f, obsdir = %d; potential = %d\n", target_pos, ret_obs_sn.idx, potential);
		// get mode: virtual target OR real target
		
		potential = -1;  // probe -> simple but works

		
		// feed to fuzzy controller to have output decision 
		//deci = FZCtrl.getDecision(obstacles_pos, potential);
		deci = FZCtrl.getDecision(target_pos, obstacles_pos, potential);

		// execute decision
		// define K value
		steering_param = deci.direction/180*PI;
		velocity_param = deci.velocity;
		}
		else
		{
		//########### CASE --NO-- OBSTACLE
		// go straight to target
			//potential = 1;
			d0_flg = 1;
			steering_param = 0;
			velocity_param = 100;
		}


		if(potential < 0 ) 
		{	
			printf("potential is negative!!!!!!!!\n"); // catch bug!if(potential < 0 ) printf("potential is negative!!!!!!!!\n"); // catch bug!
			//getchar();
		}


		// ############ SERVOING MODULE
		//printf("   steer= %0.2f;vel= %0.2f\n", steering_param, velocity_param);
		printf("   steer= %0.2f;vel= %0.2f\n", steering_param, linear_vel);
		linear_vel = linear_vel + velocity_param;
		if (abs(linear_vel) > 100) linear_vel = linear_vel/abs(linear_vel)*100;

		
		// call steering
		//steering(steering_param, velocity_param);
		robot.setVel(linear_vel);
		//robot.setDeltaHeading(5*steering_param/PI*180);


		// press "Esc" to escape
		c = (char)waitKey(1);
		if (c == 27)
		{
			break;
		}
	}  
	
//==============================editing your code here=========================

robot.waitForRunExit();
Aria::exit(0);


	waitKey(0);
	getchar();

    return 0;

} // end main()


